#include <Wire.h> 
#include <SoftwareSerial.h>
#include "LSM303.h"
#include <SD.h>

// Serial communication between GPS and Arduino
SoftwareSerial GPS(4, 5);
boolean gps_set_success = 0 ;
// SD
const int chipSelect = 10;
File dataFile;

// Serial communication between compass and Arduino
LSM303 compass;
int sens0 = 0;    //sensors
int sens1 = 1;
int sens2 = 2;
int sens3 = 3;

// Compass data (6) and analog data (4)
int data[10] = {0,0,0,0,0,0,0,0,0,0};
// Data timestamp
unsigned long time = 0;
// Time to wait between data samples (in microsecond)
unsigned long sample_interval = 50000;

// At the start of each line of data
const byte data_sentinel= B00100011; // '# for compass data line
const byte gps_sentinel = B00100100; // '$' for gps data line

void setup() {
  Serial.begin(115200); 
  //output CS or SS from SD card
  pinMode(chipSelect, OUTPUT);
  // failure check
  Serial.write(gps_sentinel);
  if (SD.begin(chipSelect)) {
    dataFile = SD.open("combined.txt", FILE_WRITE);
    if (dataFile)
      Serial.println("SD card successfully initialized.");
    else
      Serial.println("SD card datefile could not be opened.");
  }
  else {
    Serial.println("SD card failed to initialize.");
  }

  delay(2000);
  GPSsetup();
  compassSetup();
  delay(1000);
  
  // adjust the prescale for the analog inputs (http://arduino.cc/forum/index.php/topic,6549.0.html)
  
  // defines for setting and clearing register bit
  #ifndef cbi
  #define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit))
  #endif
  #ifndef sbi
  #define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit))
  #endif
  
  // set prescale to 16
  sbi(ADCSRA,ADPS2) ;
  cbi(ADCSRA,ADPS1) ;
  cbi(ADCSRA,ADPS0) ; 
}
 
void loop() {
  char GPSchar = '\0';
  while(1) {
    // If the GPS has data ready, send it out.
    if(GPS.available()) {
      // Keep the data in sync with the GPS strings (results in a more consistent number)
      time = 0;
      // So the gps data can still be recognized in case the gps string is split between multiple groups
      dataFile.write(gps_sentinel);
      Serial.write(gps_sentinel);
      do {
        // Read and write one character from GPS to serial
        GPSchar = GPS.read();
        dataFile.write(GPSchar);
        Serial.write(GPSchar);
      } while(GPS.available());
      dataFile.flush(); // Flush the SD card every second
    }
    // Control the sampling cadence
    if ((micros() - time) > sample_interval) {
      sensorRead();
      sensorSend();
    }
  }
  dataFile.close();
}

void sensorRead() {
  time = micros();
  compass.read();
  data[6] = (unsigned int)analogRead(sens0);
  data[7] = (unsigned int)analogRead(sens1);
  data[8] = (unsigned int)analogRead(sens2);
  data[9] = (unsigned int)analogRead(sens3);
  data[0] = (int)compass.a.x;
  data[1] = (int)compass.a.y;
  data[2] = (int)compass.a.z;
  data[3] = (int)compass.m.x;
  data[4] = (int)compass.m.y;
  data[5] = (int)compass.m.z;
}

void sensorSend() {
  // Write data to the SD card
  dataFile.write(data_sentinel);
  dataFile.write((byte *)(&time),4);
  dataFile.write((byte *)data,20);

  // Send data over the radio
  Serial.write(data_sentinel);
  Serial.write((byte *)(&time),4);
  Serial.write((byte *)data,20);
}

void GPSsetup() {
  GPS.begin(9600); 
  //
  // THE FOLLOWING COMMAND SWITCHES MODULE TO 4800 BAUD
  // THEN SWITCHES THE SOFTWARE SERIAL TO 4,800 BAUD
  //
  GPS.print("$PUBX,41,1,0007,0003,4800,0*13\r\n"); 
  GPS.begin(4800);
  GPS.flush();
 
  //  THIS COMMAND SETS FLIGHT MODE AND CONFIRMS IT 
  uint8_t setNav[] = {0xB5, 0x62, 0x06, 0x24, 0x24, 0x00, 0xFF, 0xFF, 0x06, 
      0x03, 0x00, 0x00, 0x00, 0x00, 0x10, 0x27, 0x00, 0x00, 0x05, 0x00, 0xFA,
      0x00, 0xFA, 0x00, 0x64, 0x00, 0x2C, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 
      0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x16, 0xDC};
  while(!gps_set_success) {
    sendUBX(setNav, sizeof(setNav)/sizeof(uint8_t));
    gps_set_success=getUBX_ACK(setNav);
  }
  gps_set_success=0;

  // Turning off all GPS NMEA strings apart from GPGGA on the uBlox modules
  GPS.print("$PUBX,40,GLL,0,0,0,0*5C\r\n");
  GPS.print("$PUBX,40,ZDA,0,0,0,0*44\r\n");
  GPS.print("$PUBX,40,VTG,0,0,0,0*5E\r\n");
  GPS.print("$PUBX,40,GSV,0,0,0,0*59\r\n");
  GPS.print("$PUBX,40,GSA,0,0,0,0*4E\r\n");
  GPS.print("$PUBX,40,RMC,0,0,0,0*47\r\n");
}

void compassSetup() {
  Wire.begin();
  compass.init(); //Turns on LSM303DLH
  compass.enableDefault();
}

// Send a byte array of UBX protocol to the GPS
void sendUBX(uint8_t *MSG, uint8_t len) {
  for(int i=0; i<len; i++) {
    GPS.write(MSG[i]);
  }
  GPS.println();
}
 
// Calculate expected UBX ACK packet and parse UBX response from GPS
boolean getUBX_ACK(uint8_t *MSG) {
  uint8_t b;
  uint8_t ackByteID = 0;
  uint8_t ackPacket[10];
  unsigned long startTime = millis();
 
  // Construct the expected ACK packet    
  ackPacket[0] = 0xB5;	// header
  ackPacket[1] = 0x62;	// header
  ackPacket[2] = 0x05;	// class
  ackPacket[3] = 0x01;	// id
  ackPacket[4] = 0x02;	// length
  ackPacket[5] = 0x00;
  ackPacket[6] = MSG[2];	// ACK class
  ackPacket[7] = MSG[3];	// ACK id
  ackPacket[8] = 0;		// CK_A
  ackPacket[9] = 0;		// CK_B
 
  // Calculate the checksums
  for (uint8_t i=2; i<8; i++) {
    ackPacket[8] = ackPacket[8] + ackPacket[i];
    ackPacket[9] = ackPacket[9] + ackPacket[8];
  }
 
  while (1) {
    // Test for success
    if (ackByteID > 9) {
      // All packets in order!
      return true;
    }
 
    // Timeout if no valid response in 3 seconds
    if (millis() - startTime > 3000) { 
      return false;
    }
 
    // Make sure data is available to read
    if (GPS.available()) {
      b = GPS.read();
 
      // Check that bytes arrive in sequence as per expected ACK packet
      if (b == ackPacket[ackByteID]) { 
        ackByteID++;
      } 
      else {
        ackByteID = 0;	// Reset and look again, invalid order
      }
    }
  }
}
